#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_cv/grid_map_cv.hpp>

#include <opencv2/opencv.hpp>

#include <iostream>

using namespace grid_map;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "grid_map_from_image");
    ros::NodeHandle nh("~");
    ros::Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);

    std::string elevation_path, dx_path, dy_path, wz_path, adj_x_path, adj_y_path;
    double resolution, lo, hi, r;
    nh.param<std::string>("elevation_path", elevation_path, "None");
    nh.param<std::string>("dx_path", dx_path, "None");
    nh.param<std::string>("dy_path", dy_path, "None");
    nh.param<std::string>("wz_path", wz_path, "None");
    nh.param<std::string>("adj_x_path", adj_x_path, "None");
    nh.param<std::string>("adj_y_path", adj_y_path, "None");
    nh.param<double>("resolution", resolution, 0.001);
    nh.param<double>("hi", hi, 1.0);
    nh.param<double>("lo", lo, 0.0);
    nh.param<double>("rate", r, 10.0);

    cv::Mat elevation_cv, dx_cv, dy_cv, wz_cv, adj_x_cv, adj_y_cv;
    elevation_cv = cv::imread(elevation_path, cv::IMREAD_UNCHANGED | cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
    if (dx_path != "None")
        dx_cv = cv::imread(dx_path, cv::IMREAD_UNCHANGED | cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
    if (dy_path != "None")
        dy_cv = cv::imread(dy_path, cv::IMREAD_UNCHANGED | cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
    if (wz_path != "None")
        wz_cv = cv::imread(wz_path, cv::IMREAD_UNCHANGED | cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
    if (adj_x_path != "None")
        adj_x_cv = cv::imread(adj_x_path, cv::IMREAD_UNCHANGED | cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
    if (adj_y_path != "None")
        adj_y_cv = cv::imread(adj_y_path, cv::IMREAD_UNCHANGED | cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);

    GridMap map({"elevation"});
    std::string elevation_layer = "elevation";
    std::string dx_layer = "dx";
    std::string dy_layer = "dy";
    std::string wz_layer = "wz";
    std::string adj_x_layer = "ax";
    std::string adj_y_layer = "ay";
    double alphaThreshold = 0.5;
    GridMapCvConverter::initializeFromImage(elevation_cv, static_cast<float>(resolution), map, Position(0.0, 0.0));
    grid_map::GridMapCvConverter::addLayerFromImage<unsigned short, 1>(elevation_cv, elevation_layer, map, static_cast<float>(lo), static_cast<float>(hi), alphaThreshold);
    if (dx_path != "None")
        grid_map::GridMapCvConverter::addLayerFromImage<unsigned short, 1>(dx_cv, dx_layer, map, -1.0, 1.0, alphaThreshold);
    if (dy_path != "None")
        grid_map::GridMapCvConverter::addLayerFromImage<unsigned short, 1>(dy_cv, dy_layer, map, -1.0, 1.0, alphaThreshold);
    if (wz_path != "None")
        grid_map::GridMapCvConverter::addLayerFromImage<unsigned short, 1>(wz_cv, wz_layer, map, static_cast<float>(lo), static_cast<float>(hi), alphaThreshold);
    if (adj_x_path != "None")
        grid_map::GridMapCvConverter::addLayerFromImage<unsigned short, 1>(adj_x_cv, adj_x_layer, map, -32.768, 32.767, alphaThreshold);
    if (adj_y_path != "None")
        grid_map::GridMapCvConverter::addLayerFromImage<unsigned short, 1>(adj_y_cv, adj_y_layer, map, -32.768, 32.767, alphaThreshold);
    // grid_map::GridMapCvConverter::addLayerFromImage<double, 1>(image, layer, map, static_cast<float>(lo), static_cast<float>(hi), alphaThreshold);
    map.setFrameId("map");

    ros::Rate rate(r);
    while (nh.ok())
    {

        // Add data to grid map.
        ros::Time time = ros::Time::now();

        // Publish grid map.
        map.setTimestamp(time.toNSec());
        grid_map_msgs::GridMap message;
        GridMapRosConverter::toMessage(map, message);
        publisher.publish(message);
        ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec());

        // Wait for next cycle.
        rate.sleep();
    }

    return 0;
}